#pragma once

#include "../core/i_blas.h"

namespace idl
{
/*Perform Cholesky-decomposition of the n x n symmetric positive definite matrix A=U'U
where U is upper triangular. Only the upper triangular part of A
is read, and the result U is also returned in the upper triangular part of A.
Note A will be destoryed after calling this routine*/
template<typename T>
inline void i_cholesky_decompose(T *A, int n)
{
  int i, ni = 0, j, ip1;
  T *Ai, *Aj, sqrtaii, rec_sqrtaii;
  /*Determine U row by row.*/
  for (i = 0; i<n; i++, ni += n)
  {
    Ai = A+ni;
    /*Determine the diagonal element*/
    sqrtaii = i_sqrt(Ai[i]);
    Ai[i] = sqrtaii;
    rec_sqrtaii = i_rec(sqrtaii);
    /*Determine the remainder of column i*/
    ip1 = i + 1;
    for (j = ip1; j<n; j++)
    { 
      Ai[j] = rec_sqrtaii * Ai[j];
    }
    /*Subtract outer product of row i from remaining sub-block of A*/
    for (j = ip1; j < n; j++)
    {
      Aj = A + n*j;
      i_sub_scaled(Ai+j, Aj+j, n-j, Ai[j]);
    } 
  }
}

/*Backsubstitute a Cholesky-decomposition U'U=A to find x such that Ax=b.*/
template<typename T>
inline void i_cholesky_backsub(const T *UU, const T *b, T *x, int n)
{
  int i, j, ni, ip1;
  const T *UUi;
  T xi, temp;
  /*backsubstitute through U'*/
  xi = b[0];
  UUi = UU;
  temp = i_div(xi, UUi[0]);
  x[0] = temp;
  for (j = 1; j<n; j++)
  { 
    x[j] = b[j] - temp*UUi[j];
  }
  ni = n;
  for (i = 1; i<n; i++, ni+=n)
  {
    xi = x[i];
    UUi = UU + ni;
    temp = i_div(xi, UUi[i]);
    x[i] = temp;
    ip1 = i + 1;
    i_sub_scaled(UUi+ip1, x+ip1, n-ip1, temp);
  }
  /*backsubstitute through U*/
  ni = n*(n-1);
  for (i = n - 1; i >= 0; i--, ni-=n)
  {
    UUi = UU + ni;
    for (xi = x[i], j = i + 1; j<n; j++)
    {
      xi -= x[j] * UUi[j];
    }
    x[i] = i_div(xi, UUi[i]);
  }
}

/*Solve nxn linear equation system Ax=b using the Cholesky-decomposition*/
template<typename T>
inline void i_solve_cholesky(T *A, const T *b, T *x, int n)
{
  i_cholesky_decompose(A, n);
  i_cholesky_backsub(A, b, x, n);
}


} /* namespace idl */